
#include "module_io.h"



// save time() value to this variable
// each time module_io_run() function executes.
uint32_t _module_io_last_run = 0;
//Difference in ms from last module_io_run() execution
uint16_t runtimediff = 0;

//Time elapsed since button was pressed
uint16_t _buttonpressedtime[BUTTON_SIZE];
//Time button was last time released
//uint32_t _buttonreleasedtime[BUTTON_SIZE];

//Create filter array for both potentiometers
uint8_t pot_filter[POTENTIOMETER_SIZE][POTFILTERSIZE];
//Average value from last runtime, to check if signal
//should be emitted
uint8_t pot_lastvalue[POTENTIOMETER_SIZE];
//Value write position on pot filters
uint8_t pot_filter_cursor[POTENTIOMETER_SIZE];
uint8_t pot_filter_full[POTENTIOMETER_SIZE];

Potentiometer adc_potselected;
uint8_t isfirstsample = 1;

uint32_t _endsoundat = 0;

void module_io_init(void) 
/* initialize module 1 */
{
	#ifdef DEBUG
	usart_write("Initializing module_io....");
	#endif
	/**
	 * Init variables
	 */
	uint8_t i,n;
	for(i=0;i<BUTTON_SIZE;i++)
	{
		_buttonpressedtime[i] = 0;
	}
	for(n=0;n<POTENTIOMETER_SIZE;n++)
	{
		for(i=0;i<POTFILTERSIZE;i++)
		{
			pot_filter[n][i] = 0;
		}
		pot_filter_cursor[n]=0;
		pot_filter_full[n]=0;
		pot_lastvalue[n]=0;
	}
    /**
     * Define buttons as inputs
	 */
	//BTN0
	DDRA &= ~(1<<PA7);
	//BTN1 & BTN2
	DDRB &= ~(1<<PB0) | ~(1<<PB1);
	//BTN3 & BTN4
	DDRC &= ~(1<<PC6) | ~(1<<PC7);
	
	//Set pull-ups
	PORTA |= (1<<PA7);
	PORTB |= (1<<PB0) | (1<<PB1);
	PORTC |= (1<<PC6) | (1<<PC7);
	
    /**
     * Define Joystick as input
     */
	DDRA &=~((1<<PA2)|(1<<PA3)|(1<<PA4)|(1<<PA5)|(1<<PA6));
	PORTA |=(1<<PA2)|(1<<PA3)|(1<<PA4)|(1<<PA5)|(1<<PA6);
	
	
	/**
	 * Setup POTA & POTB
	 */
	

	//set clock prescaler
	ADCSRA |= (1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
	
	
	ADMUX = (1<<ADLAR) | (1<<REFS0);

	_adc_select(pota);
	
	//Enable interrupt??
	ADCSRA |= (1<<ADIE) | (1<<ADATE);
	
	
	//Bootup converter and start conversion
	ADCSRA |= (1<<ADEN) | (1<<ADSC);

	/**
     * DEFINE SPEAKER
     */
	
	
	//Counter prescaler defined by CS22:0 bits
	//010 => 8  ~1000Hz --  >60 000Hz
	//011 => 32  ~245Hz --  20 000Hz  (10kHz steps at near topside!)
	#define TC2PRESCALER 8
	TCCR2 |= (0<<CS22) | (1<<CS21) | (0<<CS20);
	
	//Keep the FOC2 last!
	TCCR2 |= (1<<WGM21) | (0<<WGM20) | (0<<COM21) | (1<<COM20);
	//TCCR2 |= (1<<FOC2);
	#ifdef DEBUG
	usart_writeln("OK");
	#endif
}

void module_io_run(void)
{
	int i;
	
	// Update how long button is pressed so far
	runtimediff = time() - _module_io_last_run;
	//at least 1ms has been ellapsed..... =)
	if(runtimediff ==0)
		runtimediff++;
	
	for(i=0;i<BUTTON_SIZE;i++)
	{
		if(buttonpressed(i))
			_buttonpressedtime[i] += runtimediff;
		else
		{
			if(_buttonpressedtime[i] > 0)
			{ /* Button was released recently */
				uint16_t data[2];
				data[0] = i;
				data[1] = _buttonpressedtime[i];
				
				signal(btn_event, &data, 2);
			}
			_buttonpressedtime[i] = 0;
		}
	}
	
	//Check pot changes
	//data[0] = pot number
	//data[1] = pot value
	uint8_t data[2];
	for(i=0;i< POTENTIOMETER_SIZE;i++)
	{
		data[0] = (uint8_t) i;
		data[1] = _potaverage(i);
		if(pot_lastvalue[i] != data[1])
		{
			signal(pot_event, data, 2);
		}
		pot_lastvalue[i] = data[1];
	}
	
	//End sound if time is full
	if(time() > _endsoundat)
		_dispwm();
	
	//update this runtime.
	_module_io_last_run = time();
	
	//print_io_info();
}

/**
 * Check if button or joystick held at the moment.
 * @param button Button id. Defined in module_io.h
 * @return True/false
 */
int8_t buttonpressed(Button button)
{
	
	switch(button)
	{
		case btn0:
			return !((PINA & 1<<PA7) >> PA7);
		case btn1:
			return !((PINB & 1<<PB0) >> PB0);
		case btn2:
			return !((PINB & 1<<PB1) >> PB1);
		case btn3:
			return !((PINC & 1<<PC6) >> PC6);
		case btn4:
			return !((PINC & 1<<PC7) >> PC7);
		case joy_up:
			return !((PINA & 1<<PA3) >> PA3);
		case joy_down:
			return !((PINA & 1<<PA2) >> PA2);
		case joy_left:
			return !((PINA & 1<<PA4) >> PA4);
		case joy_right:
			return !((PINA & 1<<PA6) >> PA6);
		case joy_enter:
			return !((PINA & 1<<PA5) >> PA5);
		
	}
	return 0;
}

/**
 * Check if button has been held for longer than
 * specifiend length in ms
 * @param button Button to check
 * @param length required pressed time in ms
 */
int8_t buttonheld(Button button, uint16_t length)
{
	if(_buttonpressedtime[button] >= length)
		return 1;
	else
		return 0;
	
}

int8_t buttonstatechanged(Button button)
{
	return 0;
}

uint8_t potvalue(Potentiometer pot)
{
	//For now returns averaged raw pot values,
	//add some calibration calculations?
	return _potaverage(pot);
}

uint8_t _potaverage(Potentiometer pot)
{
	uint8_t avg = pot_filter[pot][0];
	uint8_t i;

	for(i=1; i< POTFILTERSIZE;i++)
	{
		//If filter is full, average whole filter
		//without conditions
		if(pot_filter_full[pot])
			avg = (avg+pot_filter[pot][i])/2;
		
		//If filter is not full yet, only average samples
		//wich differ from the initial 0 value
		else if(pot_filter[pot][i] != 0)
			avg = (avg+pot_filter[pot][i])/2;
	}
	
	//Weight the last sample by defined amount
	for(i=0;i<POTLASTSAMPLEWEIGHT;i++) {
		avg = (avg + (int)_potlastsample(pot))/2;
	}
	return avg;
}

uint8_t _potlastsample(Potentiometer pot)
{
	return pot_filter[pot][pot_filter_cursor[pot]];
}

/**
 * Writes the _raw_ ADC pot value to the filter array.
 * eliminates spikes by disregarding too big changes.
 */
void _potsample(Potentiometer pot, uint8_t value)
{
	
	//increment the cursor
	//because the cursor is incremented
	//beforehand, last sample can be read
	//from under the cursor position.
	pot_filter_cursor[pot]++;
	if(pot_filter_cursor[pot] >= POTFILTERSIZE)
	{
		pot_filter_cursor[pot] = 0;
		pot_filter_full[pot] = 1;
	}
	uint8_t avg = _potaverage(pot);
	if(pot_filter_full[pot] &&
		(value > avg+POTDISREGARDTHERESHOLD || value < avg-POTDISREGARDTHERESHOLD))
		return;
	
	pot_filter[pot][pot_filter_cursor[pot]] = value;
	
	
}

void _adc_select(Potentiometer pot)
{
	if(pot == pota)
	{
		ADMUX &= ~(1<<MUX0);
		adc_potselected = pota;
	}
	if(pot == potb)
	{
		ADMUX |= (1<<MUX0);
		adc_potselected = potb;
	}
}

Potentiometer _adc_selected(void)
{
	return adc_potselected;
}

/**
 * Sounds the speaker
 * @param freq Frequency in Hz
 * @param time time in ms
 */

void sound(uint16_t freq, uint16_t length)
{
	//Set Timer/Counter2 for correct freq at OC2 pin
	//OCR2 defines top value for counter. Pin toggles when
	//counter resets.
	
	// F_ocr = F_clk/(2xNx(1+OCR2))
	// F_ocr+F_ocr*OCR2 = F_clk/(2xN)
	//OCR2 = (F_clk/(2xN) - F_ocr)/F_ocr
	//OCR2 = F_clk/(2xN*F_ocr) - 1
	
	uint32_t ocr2_val = 0;
	ocr2_val = FCLK/(2 * TC2PRESCALER * (uint32_t) freq);
	
	
	if(ocr2_val > 255)
	{
		ocr2_val = 255;
	}
	OCR2 = (uint8_t) ocr2_val;
	
	//set end time for sound
	//sound is disabled at the module_io_run() function
	_endsoundat = time() + length;
	
	_enpwm();
}


ISR(ADC_vect)
{
	//change channel and take sample
	//NOTE:   Next sample after changing the adc channel
	//        still comes from the previous channel!
	if(_adc_selected()==pota)
	{
		if(!isfirstsample)
			_potsample(potb, ADCH);
		_adc_select(potb);
	}
	else
	{
		if(!isfirstsample)
			_potsample(pota, ADCH);
		_adc_select(pota);
	}
	isfirstsample = 0;
}

#ifdef MODULE_IO_UNITTEST
uint8_t module_io_unittest(void)
{
	uint32_t ctr = 0;
	sound(6000, 5000);
	while(1)
	{
		ctr++;
		if(ctr>1000)
		{
			module_io_run();
			ctr = 0;
			
			if(buttonpressed(joy_up))
				sound(5000,100);
			else if(buttonpressed(joy_down))
				_dispwm();
			else if(buttonpressed(joy_left))
				sound(200+potvalue(pota)*10,600);
			else if(buttonpressed(joy_right))
				sound(0+potvalue(potb)*100,100);
			else if(buttonheld(joy_enter, 1000))
				sound(10000, 5000);
		}
	}
	return 0;

}
#endif

void print_io_info(void)
{
// 	lcd_clear_display();
// 	lcd_clear_command_line();
// 	
	char *debug = (char *) malloc(sizeof(char)*50);
	char joydata[9];
	sprintf(debug, "POTA AVG %i RAW %i  ",potvalue(pota),_potlastsample(pota));
	lcd_write(0,debug);
	sprintf(debug, "POTB AVG %i RAW %i  ",potvalue(potb),_potlastsample(potb));
	lcd_write(1,debug);
	
	
	if(buttonpressed(joy_up))
	{
		sprintf(joydata, "UP    ");
		can_message_headlights(1);
	}
	else if(buttonpressed(joy_down))
	{
		sprintf(joydata, "DOWN  ");
		can_message_headlights(0);
	}
	else if(buttonpressed(joy_left))
		sprintf(joydata, "LEFT  ");
	else if(buttonpressed(joy_right))
		sprintf(joydata, "RIGHT  ");
	else if(buttonpressed(joy_enter))
		sprintf(joydata, "ENTER  ");
	else
		sprintf(joydata, "NONE  ");
	
	sprintf(debug, "BTN0 %i   JOYSTICK", buttonpressed(btn0));
	lcd_write(3,debug);
	sprintf(debug, "BTN1 %i     %s", buttonpressed(btn1), joydata);
	lcd_write(4,debug);
	sprintf(debug, "BTN2 %i", buttonpressed(btn2));
	lcd_write(5,debug);
	sprintf(debug, "BTN3 %i", buttonpressed(btn3));
	lcd_write(6,debug);
	sprintf(debug, "BTN4 %i", buttonpressed(btn4));
	lcd_write(7,debug);
	free(debug);
}